

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>packnet_sfm.datasets.kitti_dataset &mdash; PackNet-SfM 1.0 documentation</title>
  

  
  
  
  

  
  <script type="text/javascript" src="../../../_static/js/modernizr.min.js"></script>
  
    
      <script type="text/javascript" id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
        <script src="../../../_static/jquery.js"></script>
        <script src="../../../_static/underscore.js"></script>
        <script src="../../../_static/doctools.js"></script>
        <script src="../../../_static/language_data.js"></script>
    
    <script type="text/javascript" src="../../../_static/js/theme.js"></script>

    

  
  <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" /> 
</head>

<body class="wy-body-for-nav">

   
  <div class="wy-grid-for-nav">
    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search" >
          

          
            <a href="../../../index.html">
          

          
            
            <img src="../../../_static/logo.png" class="logo" alt="Logo"/>
          
          </a>

          
            
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
              
            
            
              <p class="caption"><span class="caption-text">Contents</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../../configs/configs.html">Configs</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../scripts/scripts.html">Scripts</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../trainers/trainers.html">Trainers</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../datasets/datasets.html">Datasets</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../models/models.html">Models</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../networks/networks.html">Networks</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../losses/losses.html">Losses</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../loggers/loggers.html">Loggers</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../geometry/geometry.html">Geometry</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../utils/utils.html">Utils</a></li>
</ul>
<p class="caption"><span class="caption-text">Contact</span></p>
<ul>
<li class="toctree-l1"><a class="reference external" href="https://tri.global">Toyota Research Institute</a></li>
<li class="toctree-l1"><a class="reference external" href="https://github.com/TRI-ML/packnet-sfm">PackNet-SfM GitHub</a></li>
<li class="toctree-l1"><a class="reference external" href="https://github.com/TRI-ML/DDAD">DDAD GitHub</a></li>
</ul>

            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" aria-label="top navigation">
        
          <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
          <a href="../../../index.html">PackNet-SfM</a>
        
      </nav>


      <div class="wy-nav-content">
        
        <div class="rst-content">
        
          















<div role="navigation" aria-label="breadcrumbs navigation">

  <ul class="wy-breadcrumbs">
    
      <li><a href="../../../index.html">Docs</a> &raquo;</li>
        
          <li><a href="../../index.html">Module code</a> &raquo;</li>
        
      <li>packnet_sfm.datasets.kitti_dataset</li>
    
    
      <li class="wy-breadcrumbs-aside">
        
      </li>
    
  </ul>

  
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <h1>Source code for packnet_sfm.datasets.kitti_dataset</h1><div class="highlight"><pre>
<span></span><span class="c1"># Copyright 2020 Toyota Research Institute.  All rights reserved.</span>

<span class="kn">import</span> <span class="nn">glob</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">os</span>

<span class="kn">from</span> <span class="nn">torch.utils.data</span> <span class="kn">import</span> <span class="n">Dataset</span>

<span class="kn">from</span> <span class="nn">packnet_sfm.datasets.kitti_dataset_utils</span> <span class="kn">import</span> \
    <span class="n">pose_from_oxts_packet</span><span class="p">,</span> <span class="n">read_calib_file</span><span class="p">,</span> <span class="n">transform_from_rot_trans</span>
<span class="kn">from</span> <span class="nn">packnet_sfm.utils.image</span> <span class="kn">import</span> <span class="n">load_image</span>
<span class="kn">from</span> <span class="nn">packnet_sfm.geometry.pose_utils</span> <span class="kn">import</span> <span class="n">invert_pose_numpy</span>

<span class="c1">########################################################################################################################</span>

<span class="n">IMAGE_FOLDER</span> <span class="o">=</span> <span class="p">{</span>
    <span class="s1">&#39;left&#39;</span><span class="p">:</span> <span class="s1">&#39;image_02&#39;</span><span class="p">,</span>
    <span class="s1">&#39;right&#39;</span><span class="p">:</span> <span class="s1">&#39;image_03&#39;</span><span class="p">,</span>
<span class="p">}</span>
<span class="n">CALIB_FILE</span> <span class="o">=</span> <span class="p">{</span>
    <span class="s1">&#39;cam2cam&#39;</span><span class="p">:</span> <span class="s1">&#39;calib_cam_to_cam.txt&#39;</span><span class="p">,</span>
    <span class="s1">&#39;velo2cam&#39;</span><span class="p">:</span> <span class="s1">&#39;calib_velo_to_cam.txt&#39;</span><span class="p">,</span>
    <span class="s1">&#39;imu2velo&#39;</span><span class="p">:</span> <span class="s1">&#39;calib_imu_to_velo.txt&#39;</span><span class="p">,</span>
<span class="p">}</span>
<span class="n">PNG_DEPTH_DATASETS</span> <span class="o">=</span> <span class="p">[</span><span class="s1">&#39;groundtruth&#39;</span><span class="p">]</span>
<span class="n">OXTS_POSE_DATA</span> <span class="o">=</span> <span class="s1">&#39;oxts&#39;</span>

<span class="c1">########################################################################################################################</span>
<span class="c1">#### FUNCTIONS</span>
<span class="c1">########################################################################################################################</span>

<div class="viewcode-block" id="read_npz_depth"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset.html#packnet_sfm.datasets.kitti_dataset.read_npz_depth">[docs]</a><span class="k">def</span> <span class="nf">read_npz_depth</span><span class="p">(</span><span class="n">file</span><span class="p">,</span> <span class="n">depth_type</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;Reads a .npz depth map given a certain depth_type.&quot;&quot;&quot;</span>
    <span class="n">depth</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">load</span><span class="p">(</span><span class="n">file</span><span class="p">)[</span><span class="n">depth_type</span> <span class="o">+</span> <span class="s1">&#39;_depth&#39;</span><span class="p">]</span><span class="o">.</span><span class="n">astype</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span><span class="n">depth</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">2</span><span class="p">)</span></div>

<div class="viewcode-block" id="read_png_depth"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset.html#packnet_sfm.datasets.kitti_dataset.read_png_depth">[docs]</a><span class="k">def</span> <span class="nf">read_png_depth</span><span class="p">(</span><span class="n">file</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;Reads a .png depth map.&quot;&quot;&quot;</span>
    <span class="n">depth_png</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">load_image</span><span class="p">(</span><span class="n">file</span><span class="p">),</span> <span class="n">dtype</span><span class="o">=</span><span class="nb">int</span><span class="p">)</span>
    <span class="k">assert</span> <span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">max</span><span class="p">(</span><span class="n">depth_png</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">255</span><span class="p">),</span> <span class="s1">&#39;Wrong .png depth file&#39;</span>
    <span class="n">depth</span> <span class="o">=</span> <span class="n">depth_png</span><span class="o">.</span><span class="n">astype</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float</span><span class="p">)</span> <span class="o">/</span> <span class="mf">256.</span>
    <span class="n">depth</span><span class="p">[</span><span class="n">depth_png</span> <span class="o">==</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="mf">1.</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">expand_dims</span><span class="p">(</span><span class="n">depth</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">2</span><span class="p">)</span></div>

<span class="c1">########################################################################################################################</span>
<span class="c1">#### DATASET</span>
<span class="c1">########################################################################################################################</span>

<div class="viewcode-block" id="KITTIDataset"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset.html#packnet_sfm.datasets.kitti_dataset.KITTIDataset">[docs]</a><span class="k">class</span> <span class="nc">KITTIDataset</span><span class="p">(</span><span class="n">Dataset</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    KITTI dataset class.</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    root_dir : str</span>
<span class="sd">        Path to the dataset</span>
<span class="sd">    file_list : str</span>
<span class="sd">        Split file, with paths to the images to be used</span>
<span class="sd">    train : bool</span>
<span class="sd">        True if the dataset will be used for training</span>
<span class="sd">    data_transform : Function</span>
<span class="sd">        Transformations applied to the sample</span>
<span class="sd">    depth_type : str</span>
<span class="sd">        Which depth type to load</span>
<span class="sd">    with_pose : bool</span>
<span class="sd">        True if returning ground-truth pose</span>
<span class="sd">    back_context : int</span>
<span class="sd">        Number of backward frames to consider as context</span>
<span class="sd">    forward_context : int</span>
<span class="sd">        Number of forward frames to consider as context</span>
<span class="sd">    strides : tuple</span>
<span class="sd">        List of context strides</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">root_dir</span><span class="p">,</span> <span class="n">file_list</span><span class="p">,</span> <span class="n">train</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span>
                 <span class="n">data_transform</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">depth_type</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">with_pose</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span>
                 <span class="n">back_context</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">forward_context</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">strides</span><span class="o">=</span><span class="p">(</span><span class="mi">1</span><span class="p">,)):</span>
        <span class="c1"># Assertions</span>
        <span class="n">backward_context</span> <span class="o">=</span> <span class="n">back_context</span>
        <span class="k">assert</span> <span class="n">backward_context</span> <span class="o">&gt;=</span> <span class="mi">0</span> <span class="ow">and</span> <span class="n">forward_context</span> <span class="o">&gt;=</span> <span class="mi">0</span><span class="p">,</span> <span class="s1">&#39;Invalid contexts&#39;</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">backward_context</span> <span class="o">=</span> <span class="n">backward_context</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">backward_context_paths</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">forward_context</span> <span class="o">=</span> <span class="n">forward_context</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">forward_context_paths</span> <span class="o">=</span> <span class="p">[]</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">with_context</span> <span class="o">=</span> <span class="p">(</span><span class="n">backward_context</span> <span class="o">!=</span> <span class="mi">0</span> <span class="ow">or</span> <span class="n">forward_context</span> <span class="o">!=</span> <span class="mi">0</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">split</span> <span class="o">=</span> <span class="n">file_list</span><span class="o">.</span><span class="n">split</span><span class="p">(</span><span class="s1">&#39;/&#39;</span><span class="p">)[</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span><span class="o">.</span><span class="n">split</span><span class="p">(</span><span class="s1">&#39;.&#39;</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">train</span> <span class="o">=</span> <span class="n">train</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">root_dir</span> <span class="o">=</span> <span class="n">root_dir</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">data_transform</span> <span class="o">=</span> <span class="n">data_transform</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span> <span class="o">=</span> <span class="n">depth_type</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">with_depth</span> <span class="o">=</span> <span class="n">depth_type</span> <span class="ow">is</span> <span class="ow">not</span> <span class="s1">&#39;&#39;</span> <span class="ow">and</span> <span class="n">depth_type</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">with_pose</span> <span class="o">=</span> <span class="n">with_pose</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">_cache</span> <span class="o">=</span> <span class="p">{}</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">pose_cache</span> <span class="o">=</span> <span class="p">{}</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">oxts_cache</span> <span class="o">=</span> <span class="p">{}</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">calibration_cache</span> <span class="o">=</span> <span class="p">{}</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">imu2velo_calib_cache</span> <span class="o">=</span> <span class="p">{}</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">sequence_origin_cache</span> <span class="o">=</span> <span class="p">{}</span>

        <span class="k">with</span> <span class="nb">open</span><span class="p">(</span><span class="n">file_list</span><span class="p">,</span> <span class="s2">&quot;r&quot;</span><span class="p">)</span> <span class="k">as</span> <span class="n">f</span><span class="p">:</span>
            <span class="n">data</span> <span class="o">=</span> <span class="n">f</span><span class="o">.</span><span class="n">readlines</span><span class="p">()</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">paths</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="c1"># Get file list from data</span>
        <span class="k">for</span> <span class="n">i</span><span class="p">,</span> <span class="n">fname</span> <span class="ow">in</span> <span class="nb">enumerate</span><span class="p">(</span><span class="n">data</span><span class="p">):</span>
            <span class="n">path</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">root_dir</span><span class="p">,</span> <span class="n">fname</span><span class="o">.</span><span class="n">split</span><span class="p">()[</span><span class="mi">0</span><span class="p">])</span>
            <span class="k">if</span> <span class="ow">not</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_depth</span><span class="p">:</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">path</span><span class="p">)</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="c1"># Check if the depth file exists</span>
                <span class="n">depth</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_depth_file</span><span class="p">(</span><span class="n">path</span><span class="p">)</span>
                <span class="k">if</span> <span class="n">depth</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">exists</span><span class="p">(</span><span class="n">depth</span><span class="p">):</span>
                    <span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">path</span><span class="p">)</span>

        <span class="c1"># If using context, filter file list</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_context</span><span class="p">:</span>
            <span class="n">paths_with_context</span> <span class="o">=</span> <span class="p">[]</span>
            <span class="k">for</span> <span class="n">stride</span> <span class="ow">in</span> <span class="n">strides</span><span class="p">:</span>
                <span class="k">for</span> <span class="n">idx</span><span class="p">,</span> <span class="n">file</span> <span class="ow">in</span> <span class="nb">enumerate</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">):</span>
                    <span class="n">backward_context_idxs</span><span class="p">,</span> <span class="n">forward_context_idxs</span> <span class="o">=</span> \
                        <span class="bp">self</span><span class="o">.</span><span class="n">_get_sample_context</span><span class="p">(</span>
                            <span class="n">file</span><span class="p">,</span> <span class="n">backward_context</span><span class="p">,</span> <span class="n">forward_context</span><span class="p">,</span> <span class="n">stride</span><span class="p">)</span>
                    <span class="k">if</span> <span class="n">backward_context_idxs</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">forward_context_idxs</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
                        <span class="n">paths_with_context</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
                        <span class="bp">self</span><span class="o">.</span><span class="n">forward_context_paths</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">forward_context_idxs</span><span class="p">)</span>
                        <span class="bp">self</span><span class="o">.</span><span class="n">backward_context_paths</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">backward_context_idxs</span><span class="p">[::</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">paths</span> <span class="o">=</span> <span class="n">paths_with_context</span>

<span class="c1">########################################################################################################################</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_get_next_file</span><span class="p">(</span><span class="n">idx</span><span class="p">,</span> <span class="n">file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get next file given next idx and current file.&quot;&quot;&quot;</span>
        <span class="n">base</span><span class="p">,</span> <span class="n">ext</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">splitext</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">basename</span><span class="p">(</span><span class="n">file</span><span class="p">))</span>
        <span class="k">return</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">dirname</span><span class="p">(</span><span class="n">file</span><span class="p">),</span> <span class="nb">str</span><span class="p">(</span><span class="n">idx</span><span class="p">)</span><span class="o">.</span><span class="n">zfill</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">base</span><span class="p">))</span> <span class="o">+</span> <span class="n">ext</span><span class="p">)</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_get_parent_folder</span><span class="p">(</span><span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get the parent folder from image_file.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">abspath</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">image_file</span><span class="p">,</span> <span class="s2">&quot;../../../..&quot;</span><span class="p">))</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_get_intrinsics</span><span class="p">(</span><span class="n">calib_data</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get intrinsics from the calib_data dictionary.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="n">calib_data</span><span class="p">[</span><span class="s1">&#39;P_rect_02&#39;</span><span class="p">],</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">4</span><span class="p">))[:,</span> <span class="p">:</span><span class="mi">3</span><span class="p">]</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_read_raw_calib_file</span><span class="p">(</span><span class="n">folder</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Read raw calibration files from folder.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">read_calib_file</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">folder</span><span class="p">,</span> <span class="n">CALIB_FILE</span><span class="p">[</span><span class="s1">&#39;cam2cam&#39;</span><span class="p">]))</span>

<span class="c1">########################################################################################################################</span>
<span class="c1">#### DEPTH</span>
<span class="c1">########################################################################################################################</span>

    <span class="k">def</span> <span class="nf">_read_depth</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">depth_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get the depth map from a file.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span> <span class="ow">in</span> <span class="p">[</span><span class="s1">&#39;velodyne&#39;</span><span class="p">]:</span>
            <span class="k">return</span> <span class="n">read_npz_depth</span><span class="p">(</span><span class="n">depth_file</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span><span class="p">)</span>
        <span class="k">elif</span> <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span> <span class="ow">in</span> <span class="p">[</span><span class="s1">&#39;groundtruth&#39;</span><span class="p">]:</span>
            <span class="k">return</span> <span class="n">read_png_depth</span><span class="p">(</span><span class="n">depth_file</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">NotImplementedError</span><span class="p">(</span>
                <span class="s1">&#39;Depth type </span><span class="si">{}</span><span class="s1"> not implemented&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span><span class="p">))</span>

    <span class="k">def</span> <span class="nf">_get_depth_file</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get the corresponding depth file from an image file.&quot;&quot;&quot;</span>
        <span class="k">for</span> <span class="n">cam</span> <span class="ow">in</span> <span class="p">[</span><span class="s1">&#39;left&#39;</span><span class="p">,</span> <span class="s1">&#39;right&#39;</span><span class="p">]:</span>
            <span class="k">if</span> <span class="n">IMAGE_FOLDER</span><span class="p">[</span><span class="n">cam</span><span class="p">]</span> <span class="ow">in</span> <span class="n">image_file</span><span class="p">:</span>
                <span class="n">depth_file</span> <span class="o">=</span> <span class="n">image_file</span><span class="o">.</span><span class="n">replace</span><span class="p">(</span>
                    <span class="n">IMAGE_FOLDER</span><span class="p">[</span><span class="n">cam</span><span class="p">]</span> <span class="o">+</span> <span class="s1">&#39;/data&#39;</span><span class="p">,</span> <span class="s1">&#39;proj_depth/</span><span class="si">{}</span><span class="s1">/</span><span class="si">{}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span>
                        <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span><span class="p">,</span> <span class="n">IMAGE_FOLDER</span><span class="p">[</span><span class="n">cam</span><span class="p">]))</span>
                <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">depth_type</span> <span class="ow">not</span> <span class="ow">in</span> <span class="n">PNG_DEPTH_DATASETS</span><span class="p">:</span>
                    <span class="n">depth_file</span> <span class="o">=</span> <span class="n">depth_file</span><span class="o">.</span><span class="n">replace</span><span class="p">(</span><span class="s1">&#39;png&#39;</span><span class="p">,</span> <span class="s1">&#39;npz&#39;</span><span class="p">)</span>
                <span class="k">return</span> <span class="n">depth_file</span>

    <span class="k">def</span> <span class="nf">_get_sample_context</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">sample_name</span><span class="p">,</span>
                            <span class="n">backward_context</span><span class="p">,</span> <span class="n">forward_context</span><span class="p">,</span> <span class="n">stride</span><span class="o">=</span><span class="mi">1</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Get a sample context</span>

<span class="sd">        Parameters</span>
<span class="sd">        ----------</span>
<span class="sd">        sample_name : str</span>
<span class="sd">            Path + Name of the sample</span>
<span class="sd">        backward_context : int</span>
<span class="sd">            Size of backward context</span>
<span class="sd">        forward_context : int</span>
<span class="sd">            Size of forward context</span>
<span class="sd">        stride : int</span>
<span class="sd">            Stride value to consider when building the context</span>

<span class="sd">        Returns</span>
<span class="sd">        -------</span>
<span class="sd">        backward_context : list of int</span>
<span class="sd">            List containing the indexes for the backward context</span>
<span class="sd">        forward_context : list of int</span>
<span class="sd">            List containing the indexes for the forward context</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">base</span><span class="p">,</span> <span class="n">ext</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">splitext</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">basename</span><span class="p">(</span><span class="n">sample_name</span><span class="p">))</span>
        <span class="n">parent_folder</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">dirname</span><span class="p">(</span><span class="n">sample_name</span><span class="p">)</span>
        <span class="n">f_idx</span> <span class="o">=</span> <span class="nb">int</span><span class="p">(</span><span class="n">base</span><span class="p">)</span>

        <span class="c1"># Check number of files in folder</span>
        <span class="k">if</span> <span class="n">parent_folder</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">_cache</span><span class="p">:</span>
            <span class="n">max_num_files</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_cache</span><span class="p">[</span><span class="n">parent_folder</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">max_num_files</span> <span class="o">=</span> <span class="nb">len</span><span class="p">(</span><span class="n">glob</span><span class="o">.</span><span class="n">glob</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">parent_folder</span><span class="p">,</span> <span class="s1">&#39;*&#39;</span> <span class="o">+</span> <span class="n">ext</span><span class="p">)))</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">_cache</span><span class="p">[</span><span class="n">parent_folder</span><span class="p">]</span> <span class="o">=</span> <span class="n">max_num_files</span>

        <span class="c1"># Check bounds</span>
        <span class="k">if</span> <span class="p">(</span><span class="n">f_idx</span> <span class="o">-</span> <span class="n">backward_context</span> <span class="o">*</span> <span class="n">stride</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">0</span> <span class="ow">or</span> <span class="p">(</span>
                <span class="n">f_idx</span> <span class="o">+</span> <span class="n">forward_context</span> <span class="o">*</span> <span class="n">stride</span><span class="p">)</span> <span class="o">&gt;=</span> <span class="n">max_num_files</span><span class="p">:</span>
            <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span>

        <span class="c1"># Backward context</span>
        <span class="n">c_idx</span> <span class="o">=</span> <span class="n">f_idx</span>
        <span class="n">backward_context_idxs</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="k">while</span> <span class="nb">len</span><span class="p">(</span><span class="n">backward_context_idxs</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">backward_context</span> <span class="ow">and</span> <span class="n">c_idx</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="n">c_idx</span> <span class="o">-=</span> <span class="n">stride</span>
            <span class="n">filename</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_next_file</span><span class="p">(</span><span class="n">c_idx</span><span class="p">,</span> <span class="n">sample_name</span><span class="p">)</span>
            <span class="k">if</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">exists</span><span class="p">(</span><span class="n">filename</span><span class="p">):</span>
                <span class="n">backward_context_idxs</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">c_idx</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">c_idx</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span>

        <span class="c1"># Forward context</span>
        <span class="n">c_idx</span> <span class="o">=</span> <span class="n">f_idx</span>
        <span class="n">forward_context_idxs</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="k">while</span> <span class="nb">len</span><span class="p">(</span><span class="n">forward_context_idxs</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">forward_context</span> <span class="ow">and</span> <span class="n">c_idx</span> <span class="o">&lt;</span> <span class="n">max_num_files</span><span class="p">:</span>
            <span class="n">c_idx</span> <span class="o">+=</span> <span class="n">stride</span>
            <span class="n">filename</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_next_file</span><span class="p">(</span><span class="n">c_idx</span><span class="p">,</span> <span class="n">sample_name</span><span class="p">)</span>
            <span class="k">if</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">exists</span><span class="p">(</span><span class="n">filename</span><span class="p">):</span>
                <span class="n">forward_context_idxs</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">c_idx</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">c_idx</span> <span class="o">&gt;=</span> <span class="n">max_num_files</span><span class="p">:</span>
            <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span>

        <span class="k">return</span> <span class="n">backward_context_idxs</span><span class="p">,</span> <span class="n">forward_context_idxs</span>

    <span class="k">def</span> <span class="nf">_get_context_files</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">sample_name</span><span class="p">,</span> <span class="n">idxs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Returns image and depth context files</span>

<span class="sd">        Parameters</span>
<span class="sd">        ----------</span>
<span class="sd">        sample_name : str</span>
<span class="sd">            Name of current sample</span>
<span class="sd">        idxs : list of idxs</span>
<span class="sd">            Context indexes</span>

<span class="sd">        Returns</span>
<span class="sd">        -------</span>
<span class="sd">        image_context_paths : list of str</span>
<span class="sd">            List of image names for the context</span>
<span class="sd">        depth_context_paths : list of str</span>
<span class="sd">            List of depth names for the context</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">image_context_paths</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">_get_next_file</span><span class="p">(</span><span class="n">i</span><span class="p">,</span> <span class="n">sample_name</span><span class="p">)</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">idxs</span><span class="p">]</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_depth</span><span class="p">:</span>
            <span class="n">depth_context_paths</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">_get_depth_file</span><span class="p">(</span><span class="n">f</span><span class="p">)</span> <span class="k">for</span> <span class="n">f</span> <span class="ow">in</span> <span class="n">image_context_paths</span><span class="p">]</span>
            <span class="k">return</span> <span class="n">image_context_paths</span><span class="p">,</span> <span class="n">depth_context_paths</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">image_context_paths</span><span class="p">,</span> <span class="kc">None</span>

<span class="c1">########################################################################################################################</span>
<span class="c1">#### POSE</span>
<span class="c1">########################################################################################################################</span>

    <span class="k">def</span> <span class="nf">_get_imu2cam_transform</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Gets the transformation between IMU an camera from an image file&quot;&quot;&quot;</span>
        <span class="n">parent_folder</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_parent_folder</span><span class="p">(</span><span class="n">image_file</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">image_file</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">imu2velo_calib_cache</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">imu2velo_calib_cache</span><span class="p">[</span><span class="n">image_file</span><span class="p">]</span>

        <span class="n">cam2cam</span> <span class="o">=</span> <span class="n">read_calib_file</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">parent_folder</span><span class="p">,</span> <span class="n">CALIB_FILE</span><span class="p">[</span><span class="s1">&#39;cam2cam&#39;</span><span class="p">]))</span>
        <span class="n">imu2velo</span> <span class="o">=</span> <span class="n">read_calib_file</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">parent_folder</span><span class="p">,</span> <span class="n">CALIB_FILE</span><span class="p">[</span><span class="s1">&#39;imu2velo&#39;</span><span class="p">]))</span>
        <span class="n">velo2cam</span> <span class="o">=</span> <span class="n">read_calib_file</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">parent_folder</span><span class="p">,</span> <span class="n">CALIB_FILE</span><span class="p">[</span><span class="s1">&#39;velo2cam&#39;</span><span class="p">]))</span>

        <span class="n">velo2cam_mat</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">velo2cam</span><span class="p">[</span><span class="s1">&#39;R&#39;</span><span class="p">],</span> <span class="n">velo2cam</span><span class="p">[</span><span class="s1">&#39;T&#39;</span><span class="p">])</span>
        <span class="n">imu2velo_mat</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">imu2velo</span><span class="p">[</span><span class="s1">&#39;R&#39;</span><span class="p">],</span> <span class="n">imu2velo</span><span class="p">[</span><span class="s1">&#39;T&#39;</span><span class="p">])</span>
        <span class="n">cam_2rect_mat</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">cam2cam</span><span class="p">[</span><span class="s1">&#39;R_rect_00&#39;</span><span class="p">],</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">))</span>

        <span class="n">imu2cam</span> <span class="o">=</span> <span class="n">cam_2rect_mat</span> <span class="o">@</span> <span class="n">velo2cam_mat</span> <span class="o">@</span> <span class="n">imu2velo_mat</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">imu2velo_calib_cache</span><span class="p">[</span><span class="n">image_file</span><span class="p">]</span> <span class="o">=</span> <span class="n">imu2cam</span>
        <span class="k">return</span> <span class="n">imu2cam</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_get_oxts_file</span><span class="p">(</span><span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Gets the oxts file from an image file.&quot;&quot;&quot;</span>
        <span class="c1"># find oxts pose file</span>
        <span class="k">for</span> <span class="n">cam</span> <span class="ow">in</span> <span class="p">[</span><span class="s1">&#39;left&#39;</span><span class="p">,</span> <span class="s1">&#39;right&#39;</span><span class="p">]:</span>
            <span class="c1"># Check for both cameras, if found replace and return file name</span>
            <span class="k">if</span> <span class="n">IMAGE_FOLDER</span><span class="p">[</span><span class="n">cam</span><span class="p">]</span> <span class="ow">in</span> <span class="n">image_file</span><span class="p">:</span>
                <span class="k">return</span> <span class="n">image_file</span><span class="o">.</span><span class="n">replace</span><span class="p">(</span><span class="n">IMAGE_FOLDER</span><span class="p">[</span><span class="n">cam</span><span class="p">],</span> <span class="n">OXTS_POSE_DATA</span><span class="p">)</span><span class="o">.</span><span class="n">replace</span><span class="p">(</span><span class="s1">&#39;.png&#39;</span><span class="p">,</span> <span class="s1">&#39;.txt&#39;</span><span class="p">)</span>
        <span class="c1"># Something went wrong (invalid image file)</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;Invalid KITTI path for pose supervision.&#39;</span><span class="p">)</span>

    <span class="k">def</span> <span class="nf">_get_oxts_data</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Gets the oxts data from an image file.&quot;&quot;&quot;</span>
        <span class="n">oxts_file</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_oxts_file</span><span class="p">(</span><span class="n">image_file</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">oxts_file</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">oxts_cache</span><span class="p">:</span>
            <span class="n">oxts_data</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">oxts_cache</span><span class="p">[</span><span class="n">oxts_file</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">oxts_data</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">loadtxt</span><span class="p">(</span><span class="n">oxts_file</span><span class="p">,</span> <span class="n">delimiter</span><span class="o">=</span><span class="s1">&#39; &#39;</span><span class="p">,</span> <span class="n">skiprows</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">oxts_cache</span><span class="p">[</span><span class="n">oxts_file</span><span class="p">]</span> <span class="o">=</span> <span class="n">oxts_data</span>
        <span class="k">return</span> <span class="n">oxts_data</span>

    <span class="k">def</span> <span class="nf">_get_pose</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">image_file</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Gets the pose information from an image file.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">image_file</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">pose_cache</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">pose_cache</span><span class="p">[</span><span class="n">image_file</span><span class="p">]</span>
        <span class="c1"># Find origin frame in this sequence to determine scale &amp; origin translation</span>
        <span class="n">base</span><span class="p">,</span> <span class="n">ext</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">splitext</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">basename</span><span class="p">(</span><span class="n">image_file</span><span class="p">))</span>
        <span class="n">origin_frame</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">dirname</span><span class="p">(</span><span class="n">image_file</span><span class="p">),</span> <span class="nb">str</span><span class="p">(</span><span class="mi">0</span><span class="p">)</span><span class="o">.</span><span class="n">zfill</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">base</span><span class="p">))</span> <span class="o">+</span> <span class="n">ext</span><span class="p">)</span>
        <span class="c1"># Get origin data</span>
        <span class="n">origin_oxts_data</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_oxts_data</span><span class="p">(</span><span class="n">origin_frame</span><span class="p">)</span>
        <span class="n">lat</span> <span class="o">=</span> <span class="n">origin_oxts_data</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">scale</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">lat</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">180.</span><span class="p">)</span>
        <span class="c1"># Get origin pose</span>
        <span class="n">origin_R</span><span class="p">,</span> <span class="n">origin_t</span> <span class="o">=</span> <span class="n">pose_from_oxts_packet</span><span class="p">(</span><span class="n">origin_oxts_data</span><span class="p">,</span> <span class="n">scale</span><span class="p">)</span>
        <span class="n">origin_pose</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">origin_R</span><span class="p">,</span> <span class="n">origin_t</span><span class="p">)</span>
        <span class="c1"># Compute current pose</span>
        <span class="n">oxts_data</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_oxts_data</span><span class="p">(</span><span class="n">image_file</span><span class="p">)</span>
        <span class="n">R</span><span class="p">,</span> <span class="n">t</span> <span class="o">=</span> <span class="n">pose_from_oxts_packet</span><span class="p">(</span><span class="n">oxts_data</span><span class="p">,</span> <span class="n">scale</span><span class="p">)</span>
        <span class="n">pose</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span><span class="p">)</span>
        <span class="c1"># Compute odometry pose</span>
        <span class="n">imu2cam</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_imu2cam_transform</span><span class="p">(</span><span class="n">image_file</span><span class="p">)</span>
        <span class="n">odo_pose</span> <span class="o">=</span> <span class="p">(</span><span class="n">imu2cam</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">inv</span><span class="p">(</span><span class="n">origin_pose</span><span class="p">)</span> <span class="o">@</span>
                    <span class="n">pose</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">inv</span><span class="p">(</span><span class="n">imu2cam</span><span class="p">))</span><span class="o">.</span><span class="n">astype</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">)</span>
        <span class="c1"># Cache and return pose</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">pose_cache</span><span class="p">[</span><span class="n">image_file</span><span class="p">]</span> <span class="o">=</span> <span class="n">odo_pose</span>
        <span class="k">return</span> <span class="n">odo_pose</span>

<span class="c1">########################################################################################################################</span>

    <span class="k">def</span> <span class="fm">__len__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Dataset length.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__getitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">idx</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Get dataset sample given an index.&quot;&quot;&quot;</span>
        <span class="c1"># Add image information</span>
        <span class="n">sample</span> <span class="o">=</span> <span class="p">{</span>
            <span class="s1">&#39;idx&#39;</span><span class="p">:</span> <span class="n">idx</span><span class="p">,</span>
            <span class="s1">&#39;filename&#39;</span><span class="p">:</span> <span class="s1">&#39;</span><span class="si">%s</span><span class="s1">_</span><span class="si">%010d</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">split</span><span class="p">,</span> <span class="n">idx</span><span class="p">),</span>
            <span class="s1">&#39;rgb&#39;</span><span class="p">:</span> <span class="n">load_image</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">]),</span>
        <span class="p">}</span>

        <span class="c1"># Add intrinsics</span>
        <span class="n">parent_folder</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_parent_folder</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">])</span>
        <span class="k">if</span> <span class="n">parent_folder</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">calibration_cache</span><span class="p">:</span>
            <span class="n">c_data</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">calibration_cache</span><span class="p">[</span><span class="n">parent_folder</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">c_data</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">_read_raw_calib_file</span><span class="p">(</span><span class="n">parent_folder</span><span class="p">)</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">calibration_cache</span><span class="p">[</span><span class="n">parent_folder</span><span class="p">]</span> <span class="o">=</span> <span class="n">c_data</span>
        <span class="n">sample</span><span class="o">.</span><span class="n">update</span><span class="p">({</span>
            <span class="s1">&#39;intrinsics&#39;</span><span class="p">:</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_intrinsics</span><span class="p">(</span><span class="n">c_data</span><span class="p">),</span>
        <span class="p">})</span>

        <span class="c1"># Add pose information if requested</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_pose</span><span class="p">:</span>
            <span class="n">sample</span><span class="o">.</span><span class="n">update</span><span class="p">({</span>
                <span class="s1">&#39;pose&#39;</span><span class="p">:</span> <span class="bp">self</span><span class="o">.</span><span class="n">_get_pose</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">]),</span>
            <span class="p">})</span>

        <span class="c1"># Add depth information if requested</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_depth</span><span class="p">:</span>
            <span class="n">sample</span><span class="o">.</span><span class="n">update</span><span class="p">({</span>
                <span class="s1">&#39;depth&#39;</span><span class="p">:</span> <span class="bp">self</span><span class="o">.</span><span class="n">_read_depth</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">_get_depth_file</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">])),</span>
            <span class="p">})</span>

        <span class="c1"># Add context information if requested</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_context</span><span class="p">:</span>
            <span class="c1"># Add context images</span>
            <span class="n">all_context_idxs</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">backward_context_paths</span><span class="p">[</span><span class="n">idx</span><span class="p">]</span> <span class="o">+</span> \
                               <span class="bp">self</span><span class="o">.</span><span class="n">forward_context_paths</span><span class="p">[</span><span class="n">idx</span><span class="p">]</span>
            <span class="n">image_context_paths</span><span class="p">,</span> <span class="n">_</span> <span class="o">=</span> \
                <span class="bp">self</span><span class="o">.</span><span class="n">_get_context_files</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">paths</span><span class="p">[</span><span class="n">idx</span><span class="p">],</span> <span class="n">all_context_idxs</span><span class="p">)</span>
            <span class="n">image_context</span> <span class="o">=</span> <span class="p">[</span><span class="n">load_image</span><span class="p">(</span><span class="n">f</span><span class="p">)</span> <span class="k">for</span> <span class="n">f</span> <span class="ow">in</span> <span class="n">image_context_paths</span><span class="p">]</span>
            <span class="n">sample</span><span class="o">.</span><span class="n">update</span><span class="p">({</span>
                <span class="s1">&#39;rgb_context&#39;</span><span class="p">:</span> <span class="n">image_context</span>
            <span class="p">})</span>
            <span class="c1"># Add context poses</span>
            <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">with_pose</span><span class="p">:</span>
                <span class="n">first_pose</span> <span class="o">=</span> <span class="n">sample</span><span class="p">[</span><span class="s1">&#39;pose&#39;</span><span class="p">]</span>
                <span class="n">image_context_pose</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">_get_pose</span><span class="p">(</span><span class="n">f</span><span class="p">)</span> <span class="k">for</span> <span class="n">f</span> <span class="ow">in</span> <span class="n">image_context_paths</span><span class="p">]</span>
                <span class="n">image_context_pose</span> <span class="o">=</span> <span class="p">[</span><span class="n">invert_pose_numpy</span><span class="p">(</span><span class="n">context_pose</span><span class="p">)</span> <span class="o">@</span> <span class="n">first_pose</span>
                                      <span class="k">for</span> <span class="n">context_pose</span> <span class="ow">in</span> <span class="n">image_context_pose</span><span class="p">]</span>
                <span class="n">sample</span><span class="o">.</span><span class="n">update</span><span class="p">({</span>
                    <span class="s1">&#39;pose_context&#39;</span><span class="p">:</span> <span class="n">image_context_pose</span>
                <span class="p">})</span>

        <span class="c1"># Apply transformations</span>
        <span class="k">if</span> <span class="bp">self</span><span class="o">.</span><span class="n">data_transform</span><span class="p">:</span>
            <span class="n">sample</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">data_transform</span><span class="p">(</span><span class="n">sample</span><span class="p">)</span>

        <span class="c1"># Return sample</span>
        <span class="k">return</span> <span class="n">sample</span></div>

<span class="c1">########################################################################################################################</span>
</pre></div>

           </div>
           
          </div>
          <footer>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2020, Toyota Research Institute (TRI)

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/rtfd/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.Navigation.enable(false);
      });
  </script>

  
  
    
   

</body>
</html>